
// Local
#include "simphysics.h"
#include "simutils.h"
#include "simphysics.h"


// Bullet
#include <btBulletDynamicsCommon.h>

#include <iostream>

SimPhysics::SimPhysics()
{
   // initPhysics();
   setupDebuggingScene(2);
}

SimPhysics::~SimPhysics()
{
    exitPhysics();
}


void
SimPhysics::initPhysics()
{
    ///collision configuration contains default setup for memory, collision setup
/*    mCollisionConfiguration = new btDefaultCollisionConfiguration();


    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
    mDispatcher = new    btCollisionDispatcher(mCollisionConfiguration);

    mBroadphase = new btDbvtBroadphase();

    ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
    btSequentialImpulseConstraintSolver* mSolver = new btSequentialImpulseConstraintSolver;

    mDynamicsWorld = new btDiscretemDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
    //mDynamicsWorld->setDebugDrawer(&gDebugDraw);

    mDynamicsWorld->setGravity(btVector3(0,-10,0));

    btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),1);
    btCollisionShape* fallShape = new btSphereShape(1);


    btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0)));
    btRigidBody::btRigidBodyConstructionInfo
            groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
    btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
    mDynamicsWorld->addRigidBody(groundRigidBody);


    btDefaultMotionState* fallMotionState =
            new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,50,0)));
    btScalar mass = 1;
    btVector3 fallInertia(0,0,0);
    fallShape->calculateLocalInertia(mass,fallInertia);
    btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
    btRigidBody* mFallRigidBody = new btRigidBody(fallRigidBodyCI);
    mDynamicsWorld->addRigidBody(mFallRigidBody);


    /*    for (int i=0 ; i<300 ; i++) {
                mDynamicsWorld->stepSimulation(1/60.f,10);

                btTransform trans;
                fallRigidBody->getMotionState()->getWorldTransform(trans);

                qDebug() << "sphere height: " << trans.getOrigin().getY();
        }*/
}


btRigidBody*
SimPhysics::localCreateRigidBody(float mass,
                                 const btTransform& startTransform,
                                 btCollisionShape* shape)
{
    // TODO -see what to name this
    return NULL;
}

void
SimPhysics::stepPhysics()
{
    //simple dynamics world doesn't handle fixed-time-stepping
    float ms = getDeltaTimeMicroseconds();

    ///step the simulation
    if (mDynamicsWorld)
    {
        mDynamicsWorld->stepSimulation(ms / 1000000.f);

        //optional but useful: debug drawing
        mDynamicsWorld->debugDrawWorld();

        btTransform trans;
        mFallRigidBody->getMotionState()->getWorldTransform(trans);

        std::cout << "sphere height: " << trans.getOrigin().getY() << "-------------\n";
    }


}

void
SimPhysics::resetScene()
{
    exitPhysics();
    initPhysics();
}


void
SimPhysics::exitPhysics()
{

    //cleanup in the reverse order of creation/initialization

    //remove the rigidbodies from the dynamics world and delete them
    int i;
    for (i=mDynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
    {
        btCollisionObject* obj = mDynamicsWorld->getCollisionObjectArray()[i];
        btRigidBody* body = btRigidBody::upcast(obj);
        if (body && body->getMotionState())
        {
            delete body->getMotionState();
        }
        mDynamicsWorld->removeCollisionObject( obj );
        delete obj;
    }

    //delete collision shapes
    for (int j=0;j<mCollisionShapes.size();j++)
    {
        btCollisionShape* shape = mCollisionShapes[j];
        delete shape;
    }
    mCollisionShapes.clear();

    delete mDynamicsWorld;

    delete mSolver;

    delete mBroadphase;

    delete mDispatcher;

    delete mCollisionConfiguration;


}

void
SimPhysics::render()
{
    // Go through all physics objects and render them
}

void
SimPhysics::setupDebuggingScene(int complexity)
{
    switch(complexity) {
    case 1:
    	setupLevel1Scene();
    	break;
    case 2:
        setupLevel2Scene();
        break;
    default:
    	;
    }
}

void
SimPhysics::setupLevel1Scene(void)
{
    // Ball falling on plane

	///collision configuration contains default setup for memory, collision setup
    mCollisionConfiguration = new btDefaultCollisionConfiguration();


    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
    mDispatcher = new btCollisionDispatcher(mCollisionConfiguration);

    mBroadphase = new btDbvtBroadphase();

    ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
    mSolver = new btSequentialImpulseConstraintSolver;

    mDynamicsWorld = new btDiscreteDynamicsWorld(mDispatcher,
                                                 mBroadphase,
                                                 mSolver,
                                                 mCollisionConfiguration);
    mDynamicsWorld->setDebugDrawer(&mDebugDrawer);

    mDynamicsWorld->setGravity(btVector3(0,-10,0));

    btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),1);
    btCollisionShape* fallShape = new btSphereShape(1);

    // Add ground
    btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0)));
    btRigidBody::btRigidBodyConstructionInfo
            groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
    btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
    mDynamicsWorld->addRigidBody(groundRigidBody);

    // Add sphere
    btDefaultMotionState* fallMotionState =
            new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,50,0)));
    btScalar mass = 1;
    btVector3 fallInertia(0,0,0);
    fallShape->calculateLocalInertia(mass, fallInertia);
    btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass, fallMotionState, fallShape, fallInertia);
    mFallRigidBody = new btRigidBody(fallRigidBodyCI);
    mDynamicsWorld->addRigidBody(mFallRigidBody);

    // DO NOT CLEAN UP HERE as debugDrawer will access the dynamics world

}

void
SimPhysics::setupLevel2Scene(void)
{
    // Ball falling on plane but collides on cube standing on the plane

	///collision configuration contains default setup for memory, collision setup
    mCollisionConfiguration = new btDefaultCollisionConfiguration();


    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
    mDispatcher = new btCollisionDispatcher(mCollisionConfiguration);

    mBroadphase = new btDbvtBroadphase();

    ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
    mSolver = new btSequentialImpulseConstraintSolver;

    mDynamicsWorld = new btDiscreteDynamicsWorld(mDispatcher,
                                                 mBroadphase,
                                                 mSolver,
                                                 mCollisionConfiguration);
    mDynamicsWorld->setDebugDrawer(&mDebugDrawer);

    mDynamicsWorld->setGravity(btVector3(0,-10,0));


    // Add ground
    btCollisionShape* groundCubeShape = new btBoxShape(btVector3(5.0, 0.5, 5.0));
	btDefaultMotionState* groundCubeMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0, -0.5, 0)));
	btRigidBody::btRigidBodyConstructionInfo
		   groundCubeRigidBodyCI(0, groundCubeMotionState, groundCubeShape, btVector3(0,0,0));
	btRigidBody* groundCubeRigidBody = new btRigidBody(groundCubeRigidBodyCI);
	mDynamicsWorld->addRigidBody(groundCubeRigidBody);

    // Add cube
    btCollisionShape* cubeShape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
	btDefaultMotionState* cubeMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(-0.5, 0.5, 0)));
	btRigidBody::btRigidBodyConstructionInfo
		   cubeRigidBodyCI(0, cubeMotionState, cubeShape, btVector3(0,0,0));
	btRigidBody* cubeRigidBody = new btRigidBody(cubeRigidBodyCI);
	mDynamicsWorld->addRigidBody(cubeRigidBody);


    // Add sphere
	btCollisionShape* fallShape = new btSphereShape(1);
    btDefaultMotionState* fallMotionState =
            new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,50,0)));
    btScalar mass = 1;
    btVector3 fallInertia(0,0,0);
    fallShape->calculateLocalInertia(mass, fallInertia);
    btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass, fallMotionState, fallShape, fallInertia);
    mFallRigidBody = new btRigidBody(fallRigidBodyCI);
    mDynamicsWorld->addRigidBody(mFallRigidBody);
}

